function [features, feature_ids] = extract_features_from_scan(scan, angles, min_range, max_range, hit_flags)
% 从激光扫描中提取特征点（改进版 - 只使用击中障碍物的点）
% 输入: scan-扫描距离, angles-扫描角度, min_range/max_range-有效范围
%       hit_flags-是否击中障碍物的标记（可选）
% 输出: features-特征点[r;φ], feature_ids-临时索引（不用于关联，实际关联由data_association完成）

    features = [];
    feature_ids = [];
    
    % 过滤无效扫描（必须在有效范围内 且 击中了障碍物）
    if nargin >= 5 && ~isempty(hit_flags)
        % 使用击中标记过滤
        valid_mask = (scan > min_range) & (scan < max_range - 0.1) & hit_flags;
    else
        % 向后兼容：只使用距离过滤
        valid_mask = (scan > min_range) & (scan < max_range - 0.1);
    end
    
    if sum(valid_mask) < 3
        return;
    end
    
    % 角点检测参数（优化：更严格的阈值，控制密度）
    threshold = 0.8;  % 距离变化阈值（米）- 提高阈值减少特征
    min_distance = 20;  % 最小角度间隔（索引差）- 避免特征过密
    min_spatial_distance = 0.5;  % 最小空间距离（米）- 防止特征在空间上过密
    
    % 查找距离突变点（角点）
    corners = [];
    last_corner_idx = -min_distance;  % 上一个角点的索引
    
    for i = 3:3:length(scan)-3  % 每3个点检测一次，减少计算量
        if ~valid_mask(i)
            continue;
        end
        
        % 与上一个角点距离太近，跳过
        if i - last_corner_idx < min_distance
            continue;
        end
        
        % 计算与相邻点的距离差（使用更大的窗口）
        diff_left = abs(scan(i) - scan(max(1, i-2)));
        diff_right = abs(scan(i) - scan(min(length(scan), i+2)));
        
        % 检测角点（距离突变）
        if diff_left > threshold || diff_right > threshold
            corners = [corners; i];
            last_corner_idx = i;
        end
    end
    
    % 如果角点太少，使用均匀采样
    if length(corners) < 5
        step = floor(length(scan) / 6);  % 进一步减少采样密度
        corners = 1:step:length(scan);
        corners = corners(valid_mask(corners))';
    end
    
    % 严格限制特征点数量（控制密度）
    max_features = 8;  % 进一步减少最大特征数
    if length(corners) > max_features
        % 均匀采样
        indices = round(linspace(1, length(corners), max_features));
        corners = corners(indices);
    end
    
    % 提取特征点（空间去重 + 密度控制）
    extracted_features = [];
    for i = 1:length(corners)
        idx = corners(i);
        
        % 特征点的极坐标
        range = scan(idx);
        bearing = angles(idx);
        
        % 转换为笛卡尔坐标（用于空间距离计算）
        feature_x = range * cos(bearing);
        feature_y = range * sin(bearing);
        
        % 空间去重：检查是否与已有特征过近
        too_close = false;
        if ~isempty(extracted_features)
            for j = 1:size(extracted_features, 2)
                existing_r = extracted_features(1, j);
                existing_b = extracted_features(2, j);
                existing_x = existing_r * cos(existing_b);
                existing_y = existing_r * sin(existing_b);
                
                % 计算空间距离
                spatial_dist = sqrt((feature_x - existing_x)^2 + (feature_y - existing_y)^2);
                
                if spatial_dist < min_spatial_distance
                    too_close = true;
                    break;
                end
            end
        end
        
        % 只添加不过密的特征
        if ~too_close
            extracted_features = [extracted_features, [range; bearing]];
        end
    end
    
    features = extracted_features;
    
    % 最终密度检查：如果特征仍然过多，进一步筛选
    if size(features, 2) > max_features
        % 保留距离最分散的特征
        indices = select_well_distributed_features(features, max_features);
        features = features(:, indices);
    end
    
    % feature_ids 保留为空数组或简单索引（兼容性）
    feature_ids = (1:size(features, 2))';  % 临时索引，不用于关联
end

function indices = select_well_distributed_features(features, max_count)
% 选择空间分布最均匀的特征子集
% 使用贪心算法：每次选择距离已选特征最远的点

    n_features = size(features, 2);
    if n_features <= max_count
        indices = 1:n_features;
        return;
    end
    
    % 转换为笛卡尔坐标
    x = features(1, :) .* cos(features(2, :));
    y = features(1, :) .* sin(features(2, :));
    
    % 第一个特征：选择距离中等的点（避免极端值）
    ranges = features(1, :);
    [~, sorted_idx] = sort(ranges);
    indices = sorted_idx(ceil(n_features/2));
    
    % 贪心选择：每次添加距离已选点最远的点
    for i = 2:max_count
        max_min_dist = -1;
        best_idx = -1;
        
        for j = 1:n_features
            if ismember(j, indices)
                continue;
            end
            
            % 计算到所有已选点的最小距离
            min_dist = inf;
            for k = 1:length(indices)
                dist = sqrt((x(j) - x(indices(k)))^2 + (y(j) - y(indices(k)))^2);
                min_dist = min(min_dist, dist);
            end
            
            % 选择最小距离最大的点（最分散）
            if min_dist > max_min_dist
                max_min_dist = min_dist;
                best_idx = j;
            end
        end
        
        if best_idx > 0
            indices = [indices, best_idx];
        end
    end
end

